/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.miniBob_2011;


import edu.wpi.first.wpilibj.*;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */

public class Drive extends SimpleRobot {
    
    Joystick leftStick;
    Joystick rightStick;
    RobotDrive drive;
    
    DigitalInput armLimit; 
    
    DriverStation ds;
    
    Victor armVictor;
    Victor leftMotorVictor;
    Victor rightMotorVictor;
    
    MotorSafetyHelper motorsafety;
    
    Servo triggerServo;
    
       
    
    public void miniBobTemplate() {
        drive = new RobotDrive(1, 2);
        ds = DriverStation.getInstance();
        leftStick = new Joystick(2);
        rightStick = new Joystick(3);
        armVictor = new Victor(3);
        leftMotorVictor = new Victor(2);
        rightMotorVictor = new Victor(1);
        armLimit = new DigitalInput(4);
        triggerServo = new Servo(5);
        
    }
    
    public void robotInit() {
        System.out.println("Robot Startup Complete");
    }
    
    public void teleopInit() {
        System.out.println("Teleop Mode On");
    }
    
    public void disabled() {
        System.out.println("Disabled Mode On");
    }
    
    public void operatorControl() {
        while (isOperatorControl() && isEnabled()) {
            drive.tankDrive(leftStick, rightStick);
        }
                    Timer.delay(0.005);
    }
    
          
    public void autonomousPeriodic() {
        if (!drive.isSafetyEnabled()) {
            drive.stopMotor();
        }
    }
    
    public void teleopPeriodic() {
        if (!drive.isSafetyEnabled()) {
            drive.stopMotor();
        }
    }
    
    public void autonomous() {
        drive.tankDrive(0, 0);
        drive.stopMotor();
    }

    
    public void teleopContinuous(
            RobotDrive drive,
            Joystick leftStick,
            Joystick rightStick,
            Victor armVictor,
            Victor leftMotorVictor,
            Victor rightMoterVictor,
            Servo triggerServo) {                           
                
        if (leftStick.getTrigger() && rightStick.getTrigger()) {
            triggerServo.set(0.3);
            
        }
        
        while (leftStick.getRawButton(2) && rightStick.getRawButton(2)) {
            armVictor.set(armVictor.get() + 0.5);
                       
        }
        
        while (armLimit.getAnalogTriggerForRouting()) {
            armVictor.set(0);
            armVictor.stopMotor();
            System.out.println("Launcher is Set");
            }
            
}
    
    public void disabledTeleop() {
        drive.tankDrive(0, 0);
        drive.stopMotor();
    }
    
    public void disabledContinuous() {
        System.out.println("RobotDisabled");
    } 
    
    public void disabledPeriodic() {
        System.out.println("Robot is Currently Disabled");
        drive.tankDrive(0, 0);
        drive.stopMotor();
    }
}

 